//
// Created by luo on 2022-10-22.
//

#ifndef ARMOR_DETECT_TOOL_H
#define ARMOR_DETECT_TOOL_H

#include<opencv2/opencv.hpp>
#include <opencv2/ml/ml.hpp>
#include <fstream>
#include <chrono>
#include "Armor_detect.h"
#include "Buff_detect.h"

using namespace std::chrono;
using namespace cv;
using namespace std;
//#define PI 3.14
class tool {

public:
    //核大小



public:
    // //颜色通道数相减，找到想要的打击的颜色
    // static inline void subtract(cv::Mat imgChannels)
    // {

    // }

    /**
  *  灯条根据x轴坐标在数组中升序排列
  * @param a     低位地址   ，     b       高位地址
  * @return bool 升序格式
   */
    static inline bool campare_lamp(RotatedRect & lamp1,RotatedRect & lamp2){
        return lamp1.center.x < lamp2.center.x;
    }

//  *  根据y轴坐标在数组中升序排列
//  * @param   lamp1 低    lamp2 高
//  * @return bool 升序格式
//  */
    static inline bool campare_armor_y(const Armor & Armor1,const Armor & Armor2){
        return Armor1.armor_center.y <Armor2.armor_center.y;
    }

//  *  根据离圆心距离最近排序
//  * @param   Circle_center
//  * @return bool 升序格式
//  */
    static inline bool campare_center_distance(const Circle_center & _Circle_center1,const Circle_center & _Circle_center2){
        return _Circle_center1.distance_to_rect <_Circle_center2.distance_to_rect;
    }

//根据装甲板面积排序
    static inline bool campare_Armor_area(const Armor &Armor1, const Armor &Armor2)
    {
        return Armor1.area < Armor2.area;
    }


//  *  根据装甲板轴坐标在数组中升序排列
//  * @param   装甲板center 低    装甲板center 高
//  * @return bool 升序格式
//  */
    static inline bool campare_Armor_center(const Armor & Armor1, const Armor & Armor2){
        return Armor1.armor_center.x <Armor2.armor_center.x;
    }


    /**
     * 找出旋转矩形的长宽比
     * @param rect 矩形
     * @param max_rrect_len是长边，min_rrect_len是短边
     * @return  长宽比率
     */
    static inline float lw_raio(cv::RotatedRect &rect) {  //长宽比率
        float max_rrect_len, min_rrect_len;
        if (rect.size.width > rect.size.height)
        {
            max_rrect_len = rect.size.width;
            min_rrect_len = rect.size.height;
        }
        else
        {
            max_rrect_len = rect.size.height;
            min_rrect_len = rect.size.width;
        }
        float lw_raio = max_rrect_len / min_rrect_len;

        return lw_raio;
    }

    /**
     * 找出正矩形的长宽比
     * @param rect 矩形
     * @param max_rrect_len是长边，min_rrect_len是短边
     * @return  长宽比率
     */
    static inline float lw_rect_raio(cv::Rect &rect) {  //长宽比率
        float max_rrect_len, min_rrect_len;
        if (rect.width > rect.height)
        {
            max_rrect_len = rect.width;
            min_rrect_len = rect.height;
        }
        else
        {
            max_rrect_len = rect.height;
            min_rrect_len = rect.width;
        }
        float lw_raio = max_rrect_len / min_rrect_len;

        return lw_raio;
    }

    /**
     * 找出矩形的极值边
     * @param rect 矩形

     * @return max_rrect_len
     */
    static inline float max_rrect_len(cv::RotatedRect &rect) {  //最大长度
        float max_rrect_len, min_rrect_len;
        if (rect.size.width > rect.size.height)
        {
            max_rrect_len = rect.size.width;
            min_rrect_len = rect.size.height;
        }
        else
        {
            max_rrect_len = rect.size.height;
            min_rrect_len = rect.size.width;

        }
        return max_rrect_len;
    }

    /*两点距离
      @param p1 点1
      @param p2 点2
      @return 距离*/
    static inline float pointDistance(const cv::Point2f &p1, const cv::Point2f &p2) {
        return std::sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
    }

    /*两点距离
      @param p1 点1
      @param p2 点2
      @return 距离*/
    static inline float pointDistance_sum(std::deque<float> &average_pointDistance) {
        float sum = 0;
        std::deque<float>::iterator iter = average_pointDistance.begin();
        while (iter != average_pointDistance.end())
            sum += *iter++;
        return sum;
    }
    /**
     * 画出目标矩形
     * @param mat 图片
     * @param rect 待画的矩形
     * @return 图片
     */
    static inline cv::Mat drawRect(cv::Mat &frame, const cv::RotatedRect &rect) {
        cv::Point2f vertices[4];
        rect.points(vertices);
        for (int i = 0; i < 4; i++) {
            cv::line(frame, vertices[i], vertices[(i + 1) % 4], cv::Scalar(0, 0, 255), 3);
        }
        return frame;
    }




    /** @brief 画X形十字线，通过中心点和半长计算四个顶点的坐标
    @param img 要作画的图像.
    @param center 中心点.
    @param d 一半长.
    @param color 颜色. */
    static void drawCross(const Mat & img, Point center, int d, const Scalar & color) {
        line(img, Point(center.x - d, center.y - d), Point(center.x + d, center.y + d), color, 4);
        line(img, Point(center.x + d, center.y - d), Point(center.x - d, center.y + d), color, 5);
    }

    static cv::Point2f calculate_newpoint(const double &a, const double &b, const float &coordinate_x, const float &coordinate_y)
    {
        Mat newangle = cv::Mat(2, 2, CV_32FC1);
        Mat  rotate_mat = cv::Mat(2, 2, CV_32FC1);
        rotate_mat.ptr<float>(0)[0] = a;
        rotate_mat.ptr<float>(0)[1] = -b;
        rotate_mat.ptr<float>(1)[0] = b;
        rotate_mat.ptr<float>(1)[1] = a;


        Mat plus_angle = cv::Mat(2, 1, CV_32FC1);
        plus_angle.ptr<float>(0)[0] = coordinate_x;
        plus_angle.ptr<float>(1)[0] = coordinate_y;

        newangle = rotate_mat * plus_angle;
        cout << "newangle" << newangle << endl;

        cv::Point2f newpoint;
        newpoint.x = newangle.at<float>(0, 0);
        newpoint.y = newangle.at<float>(0, 1);
        cout << "newpoint" << newpoint << endl;

        return newpoint;
    }

    static inline cv::Mat ElementSize_dilate(const cv::Mat &diale_mat, int &structElementSize1) {

        int structElementSize = structElementSize1;
        cv::Mat element = getStructuringElement(MORPH_RECT, Size(2 * structElementSize + 1, 2 * structElementSize + 1), Point(structElementSize, structElementSize));
        cv::dilate(diale_mat, diale_mat, element);

        return diale_mat;
    }

    static inline cv::Mat morphologyEx(int model) {
        cv::Mat element;
        if(model==10){
            Mat element1 = getStructuringElement(MORPH_RECT, Size(10, 10)); //设置内核1
            return element;
        }else if(model==15){
            Mat element2 = getStructuringElement(MORPH_RECT, Size(15, 15)); //设置内核2
            return element;
        }else if(model==7){
            Mat element3 = getStructuringElement(MORPH_RECT, Size(7, 7));	//设置内核3
            return element;
        }else if(model==3){
            Mat element4 = getStructuringElement(MORPH_RECT, Size(3, 3));	//设置内核4
            return element;
        }else if(model==25){
            Mat element5 = getStructuringElement(MORPH_RECT, Size(25, 25)); //设置内核5
            return element;
        }else if(model==5){
            Mat element6 = getStructuringElement(MORPH_RECT, Size(5, 5));	//设置内核6
            return element;
        }else if(model==6){
            Mat element7 = getStructuringElement(MORPH_RECT, Size(6, 6));	//设置内核7
            return element;
        }


    }

    /*角度转弧度
    @param angle
    */
    static inline double Radian(double angle) {
        angle = angle * PI / 180;
        return angle;
    }

    /*两点角度计算
    @param point1,point2
    */
    static inline double Point_angle(const cv::Point2f point1,const cv::Point2f point2){
        double angle = atan2(abs(point1.y - point2.y), abs(point1.x - point2.x)) * 180 / CV_PI;
        return angle;
    }


    /*坐标系角度计算
    @param m_Points1,m_Points2
    */
    static inline float Make_angle(const cv::Point2f & m_Points1,const cv::Point2f & m_Points2) {
        float coordinate_x;
        float coordinate_y;
        cv::Point2f c_coordinate;
        coordinate_x = m_Points2.x - m_Points1.x;
        coordinate_y = m_Points2.y - m_Points1.y;
        //第一象限
        if (coordinate_x > 0 && coordinate_y < 0) {
            c_coordinate.x = coordinate_x;
            c_coordinate.y = -coordinate_y;
        }
            //第二象限
        else if (coordinate_x < 0 && coordinate_y < 0)
        {
            c_coordinate.x = coordinate_x;
            c_coordinate.y = -coordinate_y;
        }
            //第三象限
        else if (coordinate_x < 0 && coordinate_y > 0)
        {
            c_coordinate.x = coordinate_x;
            c_coordinate.y = -coordinate_y;
        }
            //第四象限
        else {
            c_coordinate.x = coordinate_x;
            c_coordinate.y = -coordinate_y;
        }
        float angle = atan2(c_coordinate.y, c_coordinate.x) * 180 / CV_PI;
        //cout << "angle 11 :" << angle << endl;
        return angle;
    }


    /*写入三维数据到txt文件
    @param Point3f  三维数据点
    */
    static inline void  write_3Ddata(double angle,cv::Point3f points) {
        const char* pathName = "3D_data.txt";  // 你要创建文件的路径
        ofstream fout(pathName, ios::app);//从文件末尾开始写，防止丢失文件中原来就有的内容
        if (fout) { // 如果创建成功
            fout << points.x <<","<< points. y<<","<< points.z<< ",angle :"<<angle<< endl; // 使用与cout同样的方式进行写入
            fout.close();  // 执行完操作后关闭文件句柄
        }
    }

    /*写入数据到txt文件
    @param
    */
    static inline void  data(auto data,const char* pathName) {
        ofstream fout(pathName, ios::app);//从文件末尾开始写，防止丢失文件中原来就有的内容
        if (fout) { // 如果创建成功
            fout << data<<endl;
            fout.close();  // 执行完操作后关闭文件句柄
        }
    }

    /*写入二维数据到txt文件
    @param Point3f  二维数据点
    */
    static inline void  write_2Ddata(cv::Point2f points,string pathName) {
        //const char* pathName = "2D_data.txt";  // 你要创建文件的路径
        string pathName1="/home/qidian2021/MV_test2/script/"+pathName+".txt";
        ofstream fout(pathName1,  ios::app);//从文件末尾开始写，防止丢失文件中原来就有的内容
        if (fout) { // 如果创建成功
            fout << points.x <<","<< points.y<<","<<endl; // 使用与cout同样的方式进行写入
        }
        fout.close();  // 执行完操作后关闭文件句柄


    }
    /*写入PNP数据到txt文件
    @param  PNP数据点
    */
    static inline void  write_2Ddata_PNP(cv::Mat &PNP,string pathName,double absoluteDistance) {
        //const char* pathName = "2D_data.txt";  // 你要创建文件的路径
        string pathName1="/home/qidian2021/MV_test2/script/"+pathName+".txt";
        ofstream fout(pathName1,  ios::app);//从文件末尾开始写，防止丢失文件中原来就有的内容

        double time1 = getTickCount();
        if (fout) { 	//如果创建成功
            double x = PNP.at<double>(0, 0);
            double y = PNP.at<double>(1, 0);
            double z = PNP.at<double>(2, 0);
            fout << x <<","<< y <<","<<z<<","<<absoluteDistance<<","<<time1/ getTickFrequency()<<endl; // 使用与cout同样的方式进行写入
        }
        fout.close();  //执行完操作后关闭文件句柄

    }
    static inline void  write_absoluteDistance(string pathName,double absoluteDistance) {
        //const char* pathName = "2D_data.txt";  // 你要创建文件的路径
        string pathName1="/home/qidian2021/MV_test2/script/"+pathName+".txt";
        ofstream fout(pathName1,  ios::app);//从文件末尾开始写，防止丢失文件中原来就有的内容

        double time1 = getTickCount();
        if (fout) { 	//如果创建成功

            fout <<absoluteDistance<<","<<endl; // 使用与cout同样的方式进行写入
        }
        fout.close();  //执行完操作后关闭文件句柄

    }


    static inline void  write_lamp_subtract(double x1,double y1,double x2,double y2, string pathName) {
        //const char* pathName = "2D_data.txt";  // 你要创建文件的路径
        string pathName1="/home/qidian2021/MV_test seria/script/"+pathName+".txt";
        ofstream fout(pathName1,  ios::app);//从文件末尾开始写，防止丢失文件中原来就有的内容

        double time1 = getTickCount();
        if (fout) { 	//如果创建成功

            fout << x1 <<","<< y1 <<","<<x2<<","<<y2<<endl; // 使用与cout同样的方式进行写入
        }
        fout.close();  //执行完操作后关闭文件句柄

    }

};

#endif //ARMOR_DETECT_TOOL_H
